#include "pch.h"
#include "l2cap.h"


uint8_t packet_buffer[32];

void set_header(uint8_t handle_low, uint8_t handle_high, uint8_t length)
{
	packet_buffer[0] = handle_low;
	packet_buffer[1] = handle_high;
	packet_buffer[2] = length + 4;
	packet_buffer[3] = 0x00;
	packet_buffer[4] = length;
	packet_buffer[5] = 0x00;
	packet_buffer[6] = 0x01; /*control channel*/
	packet_buffer[7] = 0x00;
}


void get_l2cap_connection_request(l2cap_connection *connection, uint8_t **pCmd, uint8_t *size)
{
	*size = 16;
	set_header(connection->handle_low, connection->handle_high, *size - 8);
	packet_buffer[8] = 0x02;
	packet_buffer[9] = connection->current_id;
	packet_buffer[10] = *size - 12;
	packet_buffer[11] = 0x00;
	packet_buffer[12] = 0x01; /*PSM 0x0001 SDP*/
 	packet_buffer[13] = 0x00;
	packet_buffer[14] = connection->scid_low;
	packet_buffer[15] = connection->scid_high;
	*pCmd = packet_buffer;
}

void get_l2cap_connection_pending_response(l2cap_connection *connection, uint8_t **pCmd, uint8_t *size)
{
	*size = 20;
	set_header(connection->handle_low, connection->handle_high, *size-8);
	packet_buffer[8] = 0x03;
	packet_buffer[9] = connection->current_id;
	packet_buffer[10] = *size - 12;
	packet_buffer[11] = 0x00;
	packet_buffer[12] = connection->scid_low;
	packet_buffer[13] = connection->scid_high;
	packet_buffer[14] = connection->dcid_low;
	packet_buffer[15] = connection->dcid_high;
	packet_buffer[16] = 0x01; /*pending*/
	packet_buffer[17] = 0x00;
	packet_buffer[18] = 0x00;
	packet_buffer[19] = 0x00;
	*pCmd = packet_buffer;
}

void get_l2cap_connection_successful_response(l2cap_connection *connection, uint8_t **pCmd, uint8_t *size)
{
	get_l2cap_connection_pending_response(connection, pCmd, size);
	(*pCmd)[16] = 0x00; /*success*/
}


void get_l2cap_configuration_request(l2cap_connection *connection, uint8_t **pCmd, uint8_t *size)
{
	*size = 16;
	set_header(connection->handle_low, connection->handle_high, *size - 8);
	packet_buffer[8] = 0x04;
	packet_buffer[9] = connection->current_id;
	packet_buffer[10] = *size - 12;
	packet_buffer[11] = 0x00;
	packet_buffer[12] = connection->dcid_low;
	packet_buffer[13] = connection->dcid_high;
	packet_buffer[14] = 0x00; /*flags*/
	packet_buffer[15] = 0x00;
	*pCmd = packet_buffer;
}

void get_l2cap_configuration_request_mtu(l2cap_connection *connection, uint8_t **pCmd, uint8_t *size)
{
	*size = 20;
	set_header(connection->handle_low, connection->handle_high, *size - 8);
	packet_buffer[8] = 0x04;
	packet_buffer[9] = connection->current_id;
	packet_buffer[10] = *size - 12;
	packet_buffer[11] = 0x00;
	packet_buffer[12] = connection->dcid_low;
	packet_buffer[13] = connection->dcid_high;
	packet_buffer[14] = 0x00; /*flags*/
	packet_buffer[15] = 0x00;

	packet_buffer[16] = 0x01;
	packet_buffer[17] = 0x02;
	packet_buffer[18] = 0x30;
	packet_buffer[19] = 0x00;
	*pCmd = packet_buffer;
}


void get_l2cap_configuration_response(l2cap_connection *connection, uint8_t **pCmd, uint8_t *size)
{
	*size = 18;
	set_header(connection->handle_low, connection->handle_high, *size - 8);
	packet_buffer[8] = 0x05; /*configuration response*/
	packet_buffer[9] = connection->current_id;
	packet_buffer[10] = *size - 12;
	packet_buffer[11] = 0x00;
	packet_buffer[12] = connection->scid_low;
	packet_buffer[13] = connection->scid_high;
	packet_buffer[14] = 0x00; /*flags*/
	packet_buffer[15] = 0x00;
	packet_buffer[16] = 0x00; /*success*/
	packet_buffer[17] = 0x00;
	*pCmd = packet_buffer;
}